/*
 * Copyright (c) 2011, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 * 
 * - May 2012: Port to OpenSceneGraph. Mario Prats
 */

#include <ros/ros.h>
#include "osg_interactive_markers/interactive_marker_client.h"

namespace osg_interactive_markers
{

InteractiveMarkerClient::PublisherContext::PublisherContext()
{
	update_seen = false;
	init_seen = false;
	last_update_seq_num = 0;
	last_init_seq_num = 0;
	update_time_ok = true;
	initialized = false;
	last_update_time = ros::Time::now();
}

// Update the value of initialized based on previous sequence
// numbers and the number from this new update.
bool InteractiveMarkerClient::PublisherContext::checkInitWith(const visualization_msgs::InteractiveMarkerUpdate::ConstPtr& update)
{
	if( (init_seen &&
			update->type == visualization_msgs::InteractiveMarkerUpdate::UPDATE &&
			last_init_seq_num + 1 >= update->seq_num)
			||
			(init_seen &&
					update->type == visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE &&
					last_init_seq_num >= update->seq_num) )
	{
		initialized = true;
	}
	return initialized;
}

// Update the value of initialized based on previous sequence
// numbers and the number from this init message.
bool InteractiveMarkerClient::PublisherContext::checkInitWith(const visualization_msgs::InteractiveMarkerInit::ConstPtr& init)
{
	M_InteractiveMarkerUpdate::const_iterator q_it_same_seq = update_queue.find( init->seq_num );
	M_InteractiveMarkerUpdate::const_iterator q_it_seq_plus_one = update_queue.find( init->seq_num + 1 );

	if( (update_seen &&
			init->seq_num + 1 >= last_update_seq_num + 1) ||
			(q_it_seq_plus_one != update_queue.end() && q_it_seq_plus_one->second->type == visualization_msgs::InteractiveMarkerUpdate::UPDATE) ||
			(q_it_same_seq != update_queue.end() && q_it_same_seq->second->type == visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE) )
	{
		initialized = true;
	}
	return initialized;
}

void InteractiveMarkerClient::PublisherContext::enqueueUpdate(const visualization_msgs::InteractiveMarkerUpdate::ConstPtr& update)
{
	if( update->type == visualization_msgs::InteractiveMarkerUpdate::UPDATE )
	{
		update_queue[ update->seq_num ] = update;
	}
	else // else this must be a KEEP_ALIVE type.
	{
		// Keep-alive messages get queued only if there is not an update
		// queued at that sequence number already.
		if( update_queue.find( update->seq_num ) == update_queue.end() )
		{
			update_queue[ update->seq_num ] = update;
		}
	}
	update_seen = true;
	last_update_seq_num = update->seq_num;
}


InteractiveMarkerClient::InteractiveMarkerClient( InteractiveMarkerReceiver* receiver )
: cleared_( true )
, subscribed_to_init_( false )
{
	receiver_ = receiver;
}

// ROS callback notifying us of an init message (full update with all data)
void InteractiveMarkerClient::processMarkerInit(const visualization_msgs::InteractiveMarkerInit::ConstPtr& marker_init)
{
	ROS_DEBUG("InteractiveMarkerClient: %s INIT %lu",
			marker_init->server_id.c_str(),
			marker_init->seq_num);

	// get caller ID of the sending entity
	if ( marker_init->server_id.empty() )
	{
		receiver_->setStatusError( "Topic", "server_id is empty!");
	}

	M_PublisherContext::iterator context_it = publisher_contexts_.find(marker_init->server_id);

	// if this is the first message from that server, create new context
	if ( context_it == publisher_contexts_.end() )
	{
		PublisherContextPtr pc(new PublisherContext());
		context_it = publisher_contexts_.insert( std::make_pair(marker_init->server_id,pc) ).first;
	}

	PublisherContextPtr context = context_it->second;

	// If this context is already initialized, we can safely ignore this
	// init message, because the updates will give us any changes
	// present here.
	if( context->initialized )
	{
		return;
	}

	if( context->checkInitWith( marker_init ))
	{
		receiver_->processMarkerChanges( &marker_init->markers );
		cleared_ = false;

		context->last_init_seq_num = marker_init->seq_num;
		context->init_seen = true;
		context->last_update_time = ros::Time::now();

		// This update completed initialization for this context.
		receiver_->setStatusOk( context_it->first, "Initialization complete.");

		// The next update message we want is the one just after this
		// init message.  Now that this context is initialized, init
		// messages will be ignored and updates will get checked against
		// last_update_seq_num.
		context->last_update_seq_num = context->last_init_seq_num;

		// See if we are completely done initializing and can unsubscribe from init.
		maybeUnsubscribeFromInit();

		playbackUpdateQueue( context );
	}
	else if( context->update_queue.empty() )
	{
		// If we've already seen an init for this publisher but we are not
		// initialized, we must erase all markers and start again to avoid
		// processing two init messages without clearMarkers() in between.
		// 
		// TODO: If we had the ability to clear the markers from a single
		// server at a time, we could just clear the one server's worth
		// here and not have to re-init all the servers.
		if( context->init_seen )
		{
			reinit();
		}

		receiver_->processMarkerChanges( &marker_init->markers );
		cleared_ = false;

		context->last_init_seq_num = marker_init->seq_num;
		context->init_seen = true;
		context->last_update_time = ros::Time::now();
	}
	// else we must have some queued updates but they don't match up with this init, so do nothing.
}

// play back the relevant updates from the queue, if any, and clear the queue.
void InteractiveMarkerClient::playbackUpdateQueue( PublisherContextPtr& context )
{
	uint64_t next_seq_needed = context->last_init_seq_num + 1;

	M_InteractiveMarkerUpdate::iterator update_it = context->update_queue.begin();
	for( ; update_it != context->update_queue.end(); update_it++ )
	{
		visualization_msgs::InteractiveMarkerUpdate::ConstPtr update = update_it->second;

		if( update->seq_num == next_seq_needed )
		{
			applyUpdate( update, context );
			next_seq_needed++;
		}
		else if( update->seq_num < next_seq_needed )
		{
			ROS_DEBUG("Ignoring unneeded queued update number %lu, looking for %lu.", update->seq_num, next_seq_needed);
		}
		else
		{
			ROS_ERROR("Found queued update number %lu, missed %lu.", update->seq_num, next_seq_needed);
		}
	}
	context->update_queue.clear();
}

void InteractiveMarkerClient::processMarkerUpdate(const visualization_msgs::InteractiveMarkerUpdate::ConstPtr& marker_update)
{
	//  ROS_DEBUG("InteractiveMarkerClient: %s %s %lu",
	//            marker_update->server_id.c_str(),
	//            (marker_update->type == visualization_msgs::InteractiveMarkerUpdate::UPDATE ? "UPDATE" : "KEEP_ALIVE"),
	//            marker_update->seq_num);

	// get caller ID of the sending entity
	if ( marker_update->server_id.empty() )
	{
		receiver_->setStatusError( "Topic", "server_id is empty!");
	}

	M_PublisherContext::iterator context_it = publisher_contexts_.find(marker_update->server_id);

	// If we haven't seen this publisher before, we need to reset the
	// display and listen to the init topic, plus of course add this
	// publisher to our list.
	if ( context_it == publisher_contexts_.end() )
	{
		PublisherContextPtr pc(new PublisherContext());
		// Remember this update message, because we may be able to use it.
		pc->enqueueUpdate(marker_update);

		context_it = publisher_contexts_.insert( std::make_pair(marker_update->server_id,pc) ).first;

		reinit();

		// We have enqueued the update message, and we can't do anything
		// else until we get an Init message from this publisher, so return.
		return;
	}

	PublisherContextPtr context = context_it->second;

	if ( !context->initialized )
	{
		if( context->checkInitWith( marker_update ))
		{
			// This update completed initialization for this context.
			receiver_->setStatusOk( context_it->first, "Initialization complete.");

			// The next update message we want is the one just after the
			// init message.  Now that this context is initialized, init
			// messages will be ignored and updates will get checked against
			// last_update_seq_num.
			context->last_update_seq_num = context->last_init_seq_num;

			// See if we are completely done initializing and can unsubscribe from init.
			maybeUnsubscribeFromInit();
		}
		else
		{
			// We have received an update before the actual init message, so
			// just queue it up and return.
			receiver_->setStatusWarn( marker_update->server_id, "Received update or keep-alive without previous INIT message. It might be lost.");
			context->enqueueUpdate(marker_update);
			return;
		}
	}

	// From here down, we know this context is initialized and we are
	// just doing a normal update or keepalive.
	applyUpdate( marker_update, context );
}

void InteractiveMarkerClient::clear()
{
	publisher_contexts_.clear();
	reinit();
}

void InteractiveMarkerClient::unsubscribedFromInit()
{
	subscribed_to_init_ = false;
}

void InteractiveMarkerClient::reinit()
{
	if( !cleared_ )
	{
		receiver_->clearMarkers();
		cleared_ = true;
	}
	if( !subscribed_to_init_ )
	{
		subscribed_to_init_ = receiver_->subscribeToInit();
	}

	// Set all contexts to uninitialized since we are throwing away all the marker data.
	M_PublisherContext::iterator context_it;
	for( context_it = publisher_contexts_.begin(); context_it != publisher_contexts_.end(); context_it++ )
	{
		context_it->second->initialized = false;
	}
}

// If all publisher contexts are initialized, unsubscribe from the "init" topic.
void InteractiveMarkerClient::maybeUnsubscribeFromInit() {
	M_PublisherContext::iterator context_it;
	for( context_it = publisher_contexts_.begin(); context_it != publisher_contexts_.end(); context_it++ )
	{
		if( !context_it->second->initialized )
		{
			return;
		}
	}
	receiver_->unsubscribeFromInit();
	subscribed_to_init_ = false;
}

void InteractiveMarkerClient::applyUpdate( const visualization_msgs::InteractiveMarkerUpdate::ConstPtr& marker_update,
		PublisherContextPtr& context )
{
	uint64_t expected_seq_num = 0;

	switch ( marker_update->type )
	{
	case visualization_msgs::InteractiveMarkerUpdate::UPDATE:
		expected_seq_num = context->last_update_seq_num + 1;
		break;

	case visualization_msgs::InteractiveMarkerUpdate::KEEP_ALIVE:
		expected_seq_num = context->last_update_seq_num;
		break;
	}

	if ( marker_update->seq_num != expected_seq_num )
	{
		if( marker_update->seq_num < expected_seq_num )
		{
			ROS_INFO("Received sequence number %lu, less than expected sequence number %lu. Ignoring.",
					marker_update->seq_num, expected_seq_num);
			return;
		}

		// we've lost some updates
		std::ostringstream s;
		s << "Detected lost update or server restart. Resetting. Reason: Received wrong sequence number (expected: " <<
				expected_seq_num << ", received: " << marker_update->seq_num << ")";
		receiver_->setStatusError( marker_update->server_id, s.str());
		reinit();
		return;
	}

	context->last_update_seq_num = marker_update->seq_num;
	context->update_seen = true;
	context->last_update_time = ros::Time::now();

	if( marker_update->type == visualization_msgs::InteractiveMarkerUpdate::UPDATE )
	{
		receiver_->processMarkerChanges( &marker_update->markers, &marker_update->poses, &marker_update->erases );
		cleared_ = false;
	}
}

bool InteractiveMarkerClient::isPublisherListEmpty()
{
	return publisher_contexts_.empty();
}

void InteractiveMarkerClient::flagLateConnections()
{
	M_PublisherContext::iterator it;
	for ( it = publisher_contexts_.begin(); it != publisher_contexts_.end(); it++ )
	{
		PublisherContextPtr& context = it->second;
		double time_since_last_update = (ros::Time::now() - context->last_update_time).toSec();
		if ( time_since_last_update > 1.0 )
		{
			std::stringstream s;
			s << "No update received for " << (int)time_since_last_update << " seconds. Connection might be lost.";
			receiver_->setStatusWarn( it->first, s.str() );
			context->update_time_ok = false;
		}
		if ( !context->update_time_ok && time_since_last_update <= 1.0 )
		{
			receiver_->setStatusOk( it->first, "OK" );
		}
	}
}

} // namespace osg_interactive_markers
